#include "Photo.h"
#include "Camera.h"

CPhoto::CPhoto()
{
	m_image = NULL;
	m_rotationVector = cvCreateMat(1, 3, CV_32FC1);
	m_translationVector = cvCreateMat(1, 3, CV_32FC1);
	m_pCamera = NULL;
	m_cameraPosition.SetPoint(0.0f, 0.0f, 0.0f);
}

CPhoto::CPhoto(CCamera* pCamera)
{
	m_image = NULL;
	m_rotationVector = cvCreateMat(1, 3, CV_32FC1);
	m_translationVector = cvCreateMat(1, 3, CV_32FC1);
	m_pCamera = pCamera;
	m_cameraPosition.SetPoint(0.0f, 0.0f, 0.0f);
}

CPhoto::CPhoto(const CPhoto &photo)
{
	m_image = cvCloneImage(photo.m_image);

	m_rotationVector = cvCloneMat(photo.m_rotationVector);
	m_translationVector = cvCloneMat(photo.m_translationVector);

	m_pCamera = photo.m_pCamera;
	m_cameraPosition = photo.m_cameraPosition;
}

CPhoto::CPhoto(CString imageName)
{
	char pImageName[256];
	Str2Char(imageName, pImageName);
	m_image = cvLoadImage(pImageName);
	m_rotationVector = cvCreateMat(1, 3, CV_32FC1);
	m_translationVector = cvCreateMat(1, 3, CV_32FC1);
	m_pCamera = NULL;
	m_cameraPosition.SetPoint(0.0f, 0.0f, 0.0f);
}

CPhoto::~CPhoto()
{
	if (m_rotationVector)
	{
		cvReleaseMat(&m_rotationVector);
		m_rotationVector = NULL;
	}
	if (m_translationVector)
	{
		cvReleaseMat(&m_translationVector);
		m_translationVector = NULL;
	}
	m_pCamera = NULL;
	if (m_image)
	{
		cvReleaseImage(&m_image);
		m_image = NULL;
	}
}

void CPhoto::SetCamera(CCamera* pCamera)
{
	m_pCamera = pCamera;
}

void CPhoto::SetImage(CString imageName)
{
	if (m_image)
	{
		cvReleaseImage(&m_image);
		m_image = NULL;
	}
	char pImageName[256];
	Str2Char(imageName, pImageName);
	m_image = cvLoadImage(pImageName);
}

void CPhoto::Pixel2GlobalPoint3D(CPixel pixel, CPoint3D &p3D)
{
	CPoint3D cameraPoint;
	m_pCamera->Pixel2CameraPoint3D(pixel, cameraPoint);
	CameraPoint3D2GlobalPoint3D(cameraPoint, p3D);
}

void CPhoto::GlobalPoint3D2Pixel(CPoint3D p3D, CPixel &pixel)
{
	CvMat* cvPoint3D = cvCreateMat(1, 3, CV_32FC1);
	CvMat* cvPixel = cvCreateMat(1, 2, CV_32FC1);

	CV_MAT_ELEM(*cvPoint3D, float, 0, 0) = p3D.m_x;
	CV_MAT_ELEM(*cvPoint3D, float, 0, 1) = p3D.m_y;
	CV_MAT_ELEM(*cvPoint3D, float, 0, 2) = p3D.m_z;
	//	calculate pixels, i.e., fill rPixel, gPixel and bPixel
	cvProjectPoints2(
		cvPoint3D, 
		m_rotationVector, 
		m_translationVector,
		m_pCamera->GetIntrinsicMatrix(), 
		m_pCamera->GetDistortionCoeffs(), 
		cvPixel, 
		NULL, 
		NULL, 
		NULL, 
		NULL, 
		NULL, 
		0
	);
	pixel.m_x = (int)CV_MAT_ELEM(*cvPixel, float, 0, 0);
	pixel.m_y = (int)CV_MAT_ELEM(*cvPixel, float, 0, 1);

	cvReleaseMat(&cvPoint3D);
	cvReleaseMat(&cvPixel);
}

void CPhoto::Pixel2Color(CPixel pixel, CColor &color)
{
	uchar* pImageData = (uchar *)(m_image->imageData + pixel.m_y * m_image->widthStep);
	color.m_b = (int)pImageData[3 * pixel.m_x];
	color.m_g = (int)pImageData[3 * pixel.m_x + 1];
	color.m_r = (int)pImageData[3 * pixel.m_x + 2];
}

void CPhoto::Pixel2Color(int x, int y, int &r, int &g, int &b)
{
	uchar* pImageData = (uchar *)(m_image->imageData + y * m_image->widthStep);
	b = (int)pImageData[3 * x];
	g = (int)pImageData[3 * x + 1];
	r = (int)pImageData[3 * x + 2];

}

void CPhoto::GlobalPoint3D2Color(CPoint3D p3D, CColor &color)
{
	CPixel pixel;
	GlobalPoint3D2Pixel(p3D, pixel);
	Pixel2Color(pixel, color);
}

void CPhoto::CameraPoint3D2GlobalPoint3D(CPoint3D cameraPoint, CPoint3D &gloPoint)
{
	CvMat* rotationMatrix = cvCreateMat(3, 3, CV_32FC1);
	CvMat* cvCameraPoint = cvCreateMat(1, 3, CV_32FC1);
	CV_MAT_ELEM(*cvCameraPoint, float, 0, 0) = cameraPoint.m_x;
	CV_MAT_ELEM(*cvCameraPoint, float, 0, 1) = cameraPoint.m_y;
	CV_MAT_ELEM(*cvCameraPoint, float, 0, 2) = cameraPoint.m_z;
	
	CvMat* v1 = cvCreateMat(1, 3, CV_32FC1);
	CvMat* cvGloPoint = cvCreateMat(1, 3, CV_32FC1);

	cvRodrigues2(m_rotationVector, rotationMatrix, NULL);
	cvSub(cvCameraPoint, m_translationVector, v1, NULL);
	cvGEMM(v1, rotationMatrix, 1, NULL, 0, cvGloPoint, 0);
	gloPoint.m_x = CV_MAT_ELEM(*cvGloPoint, float, 0, 0);
	gloPoint.m_y = CV_MAT_ELEM(*cvGloPoint, float, 0, 1);
	gloPoint.m_z = CV_MAT_ELEM(*cvGloPoint, float, 0, 2);

	cvReleaseMat(&rotationMatrix);
	cvReleaseMat(&cvCameraPoint);
	cvReleaseMat(&v1);
	cvReleaseMat(&cvGloPoint);
}

void CPhoto::GlobalPoint3D2CameraPoint3D(CPoint3D gloPoint, CPoint3D &cameraPoint)
{
	CvMat* rotationMatrix = cvCreateMat(3, 3, CV_32FC1);
	CvMat* cvGloPoint = cvCreateMat(1, 3, CV_32FC1);
	CV_MAT_ELEM(*cvGloPoint, float, 0, 0) = gloPoint.m_x;
	CV_MAT_ELEM(*cvGloPoint, float, 0, 1) = gloPoint.m_y;
	CV_MAT_ELEM(*cvGloPoint, float, 0, 2) = gloPoint.m_z;

	CvMat* v1 = cvCreateMat(1, 3, CV_32FC1);
	CvMat* cvCameraPoint = cvCreateMat(1, 3, CV_32FC1);

	cvRodrigues2(m_rotationVector, rotationMatrix, NULL);

	cvGEMM(cvGloPoint, rotationMatrix, 1, m_translationVector, 1, cvCameraPoint, CV_GEMM_B_T);

	cameraPoint.m_x = CV_MAT_ELEM(*cvCameraPoint, float, 0, 0);
	cameraPoint.m_y = CV_MAT_ELEM(*cvCameraPoint, float, 0, 1);
	cameraPoint.m_z = CV_MAT_ELEM(*cvCameraPoint, float, 0, 2);

	cvReleaseMat(&rotationMatrix);
	cvReleaseMat(&cvCameraPoint);
	cvReleaseMat(&v1);
	cvReleaseMat(&cvGloPoint);
}

void CPhoto::CalcExtrinsicParas(vector<CPixel> pixels, vector<CPoint3D> p3Ds)
{
	int n = pixels.size();
	CvMat* cvPoint3D = cvCreateMat(n, 3, CV_32FC1);
	CvMat* cvPixels = cvCreateMat(n, 2, CV_32FC1);

	for (int i = 0; i < n; i++)
	{
		CV_MAT_ELEM(*cvPoint3D, float, i, 0) = p3Ds[i].m_x;
		CV_MAT_ELEM(*cvPoint3D, float, i, 1) = p3Ds[i].m_y;
		CV_MAT_ELEM(*cvPoint3D, float, i, 2) = p3Ds[i].m_z;

		CV_MAT_ELEM(*cvPixels, float, i, 0) = (float)pixels[i].m_x;
		CV_MAT_ELEM(*cvPixels, float, i, 1) = (float)pixels[i].m_y;
	}
	
	cvFindExtrinsicCameraParams2(
		cvPoint3D, 
		cvPixels, 
		m_pCamera->GetIntrinsicMatrix(),
		m_pCamera->GetDistortionCoeffs(),
		m_rotationVector,
		m_translationVector
	);

	cvReleaseMat(&cvPoint3D);
	cvReleaseMat(&cvPixels);
	CPoint3D p3D;
	p3D.SetPoint(0.0f, 0.0f, 0.0f);
	CameraPoint3D2GlobalPoint3D(p3D, m_cameraPosition);
}

int CPhoto::GetHeight()
{
	return m_image->height;
}

int CPhoto::GetWidth()
{
	return m_image->width;
}

void CPhoto::SetPixelColor(int x, int y, int r, int g, int b)
{
	uchar* pImageData = (uchar *)(m_image->imageData + y * m_image->widthStep);
	if (r > 255)
		r = 255;
	if (g > 255)
		g = 255;
	if (b > 255)
		b = 255;
	if (r < 0)
		r = 0;
	if (g < 0)
		g = 0;
	if (b < 0)
		b = 0;
	pImageData[3 * x] = (uchar)b;
	pImageData[3 * x + 1] = (uchar)g;
	pImageData[3 * x + 2] = (uchar)r;

}

void CPhoto::SavePhoto(char* filename)
{
	cvSaveImage(filename, m_image);
}